/*
 * ahrsfilter.cpp
 *
 *  Created on: Dec 11, 2012
 *      Author: mark
 */

#include "ahrsfilter.h"

AHRSFilter::AHRSFilter() {
	SEq_1	=	1;
	SEq_2	=	0;
	SEq_3	=	0;
	SEq_4	=	0;
	beta	=	sqrt(3 / 4) * (M_PI * (gyroMeasError / 180));

	// Create the IIR filters
	double arg1[] = {
			0.999975456909767,
			-0.999975456909767
	};

	double arg2[] = {
			1,
			-0.999950913819534
	};

	HPfilterq = new IIRFilter(arg1, arg2);
	HPfilterp = new IIRFilter(arg1, arg2);
	HPfilterr = new IIRFilter(arg1, arg2);
}

AHRSFilter::~AHRSFilter() {
}

Quaternions AHRSFilter::filterupdate(double w_x, double w_y, double w_z, double a_x, double a_y, double a_z) {
	// Local system variables
	double	norm;
	double	SEqDot_omega_1,		SEqDot_omega_2,		SEqDot_omega_3,		SEqDot_omega_4;
	double	f_1,				f_2,				f_3;
	double	J_11or24,			J_12or23,			J_13or22,			J_14or21,		J_32,		J_33;
	double	nablaf_1,			nablaf_2,			nablaf_3,			nablaf_4;

	// Auxilary variables (used to avoid repeated calculations)
	double	halfSEq_1,			halfSEq_2,			halfSEq_3,			halfSEq_4;
	double	twoSEq_1,			twoSEq_2,			twoSEq_3;

	// Define the auxilary variables
	halfSEq_1	=	0.5 *	SEq_1;
	halfSEq_2	=	0.5 *	SEq_2;
	halfSEq_3	=	0.5 *	SEq_3;
	halfSEq_4	=	0.5 *	SEq_4;
	twoSEq_1	=	2	*	SEq_1;
	twoSEq_2	=	2	*	SEq_2;
	twoSEq_3	=	2	*	SEq_3;

	// Compute the quaternion rate measured by the gyroscopes
	SEqDot_omega_1	=	-halfSEq_2	*	w_x		-	halfSEq_3	*	w_y		-	halfSEq_4	*	w_z;
	SEqDot_omega_2	=	halfSEq_1	*	w_x		+	halfSEq_3	*	w_z		-	halfSEq_4	*	w_y;
	SEqDot_omega_3	=	halfSEq_1	*	w_y		-	halfSEq_2	*	w_z		+	halfSEq_4	*	w_x;
	SEqDot_omega_4	=	halfSEq_1	*	w_z		+	halfSEq_2	*	w_y		-	halfSEq_3	*	w_x;

	// Normalize the accelerometer measurement
	norm	=	sqrt(a_x * a_x + a_y * a_y + a_z * a_z);
	a_x		/=	norm;
	a_y		/=	norm;
	a_z		/=	norm;

	// Compute the objective function and Jacobian
	f_1		=	twoSEq_2	*	SEq_4		-	twoSEq_1	*	SEq_3		-	a_x;
	f_2		=	twoSEq_1	*	SEq_2		+	twoSEq_3	*	SEq_4		-	a_y;
	f_3		=	1			-	twoSEq_2	*	SEq_2		-	twoSEq_3	*	SEq_3	-	a_z;

	J_11or24	=	twoSEq_3;
	J_12or23	=	2	*	SEq_4;
	J_13or22	=	twoSEq_1;
	J_14or21	=	twoSEq_2;
	J_32		=	2	*	J_14or21;
	J_33		=	2	*	J_11or24;

	// Compute the gradient (matrix multiplication)
	nablaf_1	=	J_14or21	*	f_2		-	J_11or24	*	f_1;
	nablaf_2	=	J_12or23	*	f_1		+	J_13or22	*	f_2		-	J_32		*	f_3;
	nablaf_3	=	J_12or23	*	f_2		-	J_33		*	f_3		-	J_13or22	*	f_1;
	nablaf_4	=	J_14or21	*	f_1		+	J_11or24	*	f_2;

	// Normalise the gradient
	norm		=	sqrt(nablaf_1 * nablaf_1 + nablaf_2 * nablaf_2 + nablaf_3 * nablaf_3 + nablaf_4 * nablaf_4);
	nablaf_1	/=	norm;
	nablaf_2	/=	norm;
	nablaf_3	/=	norm;
	nablaf_4	/=	norm;

	// Compute then integrate the estimated quaternion rate
	SEq_1	+=	(SEqDot_omega_1		-	(beta	*	nablaf_1)	*	deltat);
	SEq_2	+=	(SEqDot_omega_2		-	(beta	*	nablaf_2)	*	deltat);
	SEq_3	+=	(SEqDot_omega_3		-	(beta	*	nablaf_3)	*	deltat);
	SEq_4	+=	(SEqDot_omega_4		-	(beta	*	nablaf_4)	*	deltat);

	// Normalise the quaternion
	norm	=	sqrt(SEq_1 * SEq_1 + SEq_2 * SEq_2 + SEq_3 * SEq_3 + SEq_4 * SEq_4);
	SEq_1	/=	norm;
	SEq_2	/=	norm;
	SEq_3	/=	norm;
	SEq_4	/=	norm;

	// Construct and return the quaterions
	Quaternions retval;
	retval.w	=	SEq_2;
	retval.x	=	SEq_4;
	retval.y	=	SEq_3;
	retval.z	=	SEq_1;
	return retval;
}




































































